package com.neuronrobotics.openigt;

import org.medcare.igtl.messages.GetCapabilityMessage;
import org.medcare.igtl.messages.ImageMessage;
import org.medcare.igtl.messages.OpenIGTMessage;
import org.medcare.igtl.messages.PositionMessage;
import org.medcare.igtl.messages.StatusMessage;
import org.medcare.igtl.messages.TransformMessage;
import org.medcare.igtl.network.MessageHandler;
import org.medcare.igtl.network.ServerThread;
import org.medcare.igtl.util.Header;

import com.neuronrobotics.aim.gui.ImageControlPanel;

public class MyMessageHandler extends MessageHandler {
	public OpenIGTMessage openIGTMessage;
	//private ProstateRobotKinematicModel model;
	
	public MyMessageHandler(Header header, byte[] body,
		ServerThread serverThread) {
		super(header, body, serverThread);
		capabilityList.add("GET_CAPABIL");
		capabilityList.add("TRANSFORM");
		capabilityList.add("POSITION");
		capabilityList.add("IMAGE");
		capabilityList.add("STATUS");
		capabilityList.add("MOVE_TO");
		/*// important commands
		capabilityList.add("INITIALIZE");
		capabilityList.add("HOME");
		capabilityList.add("GET_COORDINATE");
		capabilityList.add("GET_STATUS");
		// commands to be implemented
		capabilityList.add("SET_Z_FRAME");
		capabilityList.add("START_UP");
		capabilityList.add("TARGETING "); 
		capabilityList.add("INSERT"); 
		capabilityList.add("BIOPSY"); 
		capabilityList.add("EMERGENCY");*/				
		}

	@Override 
	public void manageError(String message, Exception exception, int errorCode) {
		serverThread.errorManager.error(message, exception, errorCode);
	}

	@Override
	public boolean perform(String messageType) throws Exception {
        System.out.println("perform messageType : " + messageType);
        if (messageType.equals("GET_CAPABIL")) {
                openIGTMessage = new GetCapabilityMessage(header, body);
        } else if  (messageType.equals("TRANSFORM")) {
                openIGTMessage = new TransformMessage(header, body);               
        		TransformMessage transfm = (TransformMessage) openIGTMessage;
        		transfm.Unpack();
        		        
        		/*System.out.println("Device name is"+ transfm.header.getDeviceName());        		
        		System.out.println("Device name is the same as ProstateNavRobotTarg??");
        		System.out.println(transfm.header.getDeviceName()=="ProstateNavRobotTarg");          		
        		System.out.println(transfm.header.getDeviceName()=="ProstateNavRobotTarget");    
        		        		
        		System.out.println("Device name is equal to ProstateNavRobotTarg??");
        		System.out.println(transfm.header.getDeviceName().equals("ProstateNavRobotTarg") );          		
        		*/
        		double[] position = transfm.GetOrigin();
        		double[][] rotation=transfm.GetNormals(); 		
        		
        		if ((OpenIGTPanel.networkControlEnabled==true)&&(transfm.header.getDeviceName().equals("ProstateNavRobotTarg"))){
        		ImageControlPanel.tableDesiredNeedleRAS.setTableData(rotation, position);
        		System.err.println("received image target");
        		}	
        		
        		if ((OpenIGTPanel.networkControlEnabled==true)&&(transfm.header.getDeviceName().equals("ZFrameTransform"))){
            		ImageControlPanel.tableZFrameRegistration.setTableData(rotation, position);
            		System.err.println("received Z frame transform");
            		}	
        		
        } else if (messageType.equals("POSITION")) {
                System.out.println("perform trouve POSITION");
                openIGTMessage= new PositionMessage(header, body);
        } else if (messageType.equals("IMAGE")) {
                openIGTMessage = new ImageMessage(header, body);
        } else if (messageType.equals("STATUS")) {
                openIGTMessage = new StatusMessage(header, body);
        } else {
                System.out.println("Perform messageType : " + messageType + " not implemented");
                return false;
        }
      //  System.out.println("Perform messageType : " + messageType + " content : " + openIGTMessage.toString());
        /*PositionMessage positionMessage = new PositionMessage("Traker");
        double[] quaternion = {0.0, 0.6666666666, 0.577350269189626, 0.6666666666};
        int quaternionSize = PositionMessage.ALL;
        double[] position = {0.0, 50.0, 50.0};
        positionMessage.setPositionData(position, quaternion, quaternionSize);
        System.out.println("perform messageType SendBytes");
        serverThread.sendBytes(positionMessage.PackBody());
        System.out.println("perform messageType SendBytes done");*/
        return true;
	}
	
	public void printDoubleDataArray(double[][] matrixArray) {
		for (int i = 0; i < matrixArray.length; i++) {
			for (int j = 0; j < matrixArray[0].length; j++) {
				System.out.print(matrixArray[i][j] + " ");
			}
			System.out.println("\n");
		}
	}
}
